#ifndef __DVC_DJIMOTOR_H__
#define __DVC_DJIMOTOR_H__

/*=============================Include========================================*/
#include "alg_pid.h"
#include "drv_can.h"
/*==============================ExportedMacros================================*/
#define RPM_TO_OMEGA (0.1047197551f)    // (1/60*2*pi) (rpm)->(rad/s)
#define CELSIUS_TO_KELVIN (273.15f)
#define DEGREE_TO_RAD (0.0174532925f)   // (pi/180) (degree)->(rad)
#define RAD_TO_DEGREE (57.2957795131f)  // (180/pi) (rad)->(degree)

// 电机编码值规整 0—8191
#define ecd_format(ecd)         \
	{                           \
		if ((ecd) > ECD_RANGE)  \
			(ecd) -= ECD_RANGE; \
		else if ((ecd) < 0)     \
			(ecd) += ECD_RANGE; \
	}

#define DJI_GM6020_ECD_TO_RAD \
	0.000766990394f  // (2*pi/8192) 电机编码器值转换为弧度
#define DJI_GM3508_RPM_TO_OMEGA \
	0.0055115661f  // (1/60*2*pi/19) m3508(减速比19:1) (rpm)->(rad/s)
#define DJI_GM2006_RPM_TO_OMEGA \
	0.0029088821f  // (1/60*2*pi/36) m2006(减速比36:1) (rpm)->(rad/s)

/*=============================ExportedTypes==================================*/
typedef enum dji_id {
	DJI_ID_UNDEFINED = 0,
	DJI_ID_0x201,
	DJI_ID_0x202,
	DJI_ID_0x203,
	DJI_ID_0x204,
	DJI_ID_0x205,
	DJI_ID_0x206,
	DJI_ID_0x207,
	DJI_ID_0x208,
	DJI_ID_0x209,
	DJI_ID_0x20A,
	DJI_ID_0x20B,
} dji_id_e;

typedef enum dji_control {
	OPENLOOP_CTRL = 0,
	TORQUE_CTRL,
	OMEGA_CTRL,
	ANGLE_CTRL,
} dji_control_e;

typedef struct motor_measure {
	uint16_t ecd;
	int16_t rpm;
	int16_t current;
	uint8_t temperature;
	int8_t reserved;
} __packed motor_measure_t;

typedef struct motor_data {
	float angle;
	float omega;
	float torque;
	float temperature;
	uint32_t pre_encoder;
	int32_t total_encoder;
	int32_t total_round;
} __packed motor_data_t;

class GM6020 {
   public:
	PID pid_angle;
	PID pid_omega;
	PID pid_torque;

	void init(CAN_HandleTypeDef *_hcan, dji_id_e _id,
	          dji_control_e _method = ANGLE_CTRL, int32_t _encoder_offset = 0);

	inline float get_angle();
	inline float get_omega();
	inline float get_torque();
	inline uint16_t get_temperature();
	inline float get_target_angle();
	inline float get_target_omega();
	inline float get_target_torque();
	inline float get_out();
	inline uint16_t get_out_max();

	inline void set_target_angle(float _target_angle);
	inline void set_target_omega(float _target_omega);
	inline void set_target_torque(float _target_torque);
	inline void set_out(float _out);

	void pid_calc();
	void CAN_RxCpltCallback(uint8_t *rx_data);

   protected:
	can_manager_t *can_manager;
	dji_id_e id;

	uint8_t *can_tx_data;

	uint32_t encoder_offset;
	uint16_t encoder_num_pre_round = 8192;
	uint16_t out_max = 30000;

	dji_control_e method = ANGLE_CTRL;
	motor_data_t data;

	float target_angle = 0.0f;
	float target_omega = 0.0f;
	float target_torque = 0.0f;
	float out = 0.0f;

	void data_pross();
	void output();
};

class M3508 {
   public:
	PID pid_angle;
	PID pid_omega;
	PID pid_torque;

	void init(CAN_HandleTypeDef *_hcan, dji_id_e _id,
	          dji_control_e _method = OMEGA_CTRL);

	inline float get_angle();
	inline float get_omega();
	inline float get_torque();
	inline uint16_t get_temperature();
	inline float get_target_angle();
	inline float get_target_omega();
	inline float get_target_torque();
	inline float get_out();
	inline uint16_t get_out_max();

	inline void set_target_angle(float _target_angle);
	inline void set_target_omega(float _target_omega);
	inline void set_target_torque(float _target_torque);
	inline void set_out(float _out);

	void pid_calc();
	void CAN_RxCpltCallback(uint8_t *rx_data);

   protected:
	can_manager_t *can_manager;
	dji_id_e id;

	uint8_t *can_tx_data;

	uint16_t reduction_ratio = 19;
	uint16_t out_max = 10000;

	uint16_t encoder_num_pre_round = 8192;
	dji_control_e method = OMEGA_CTRL;
	motor_data_t data;

	float target_angle = 0.0f;
	float target_omega = 0.0f;
	float target_torque = 0.0f;
	float out = 0.0f;

	void data_pross();
	void output();
};
/*=============================ExportedDefinitions============================*/

/*=============================ExportedFunctionDeclarations===================*/
/*GM6020*/
float GM6020::get_angle() { return data.angle; }
float GM6020::get_omega() { return data.omega; }
float GM6020::get_torque() { return data.torque; }
uint16_t GM6020::get_temperature() { return data.temperature; }
float GM6020::get_target_angle() { return target_angle; }
float GM6020::get_target_omega() { return target_omega; }
float GM6020::get_target_torque() { return target_torque; }
float GM6020::get_out() { return out; }
uint16_t GM6020::get_out_max() { return out_max; }

void GM6020::set_target_angle(float _target_angle)
{
	target_angle = _target_angle;
}
void GM6020::set_target_omega(float _target_omega)
{
	target_omega = _target_omega;
}
void GM6020::set_target_torque(float _target_torque)
{
	target_torque = _target_torque;
}
void GM6020::set_out(float _out) { out = _out; }

/*M3508*/
float M3508::get_angle() { return data.angle; }
float M3508::get_omega() { return data.omega; }
float M3508::get_torque() { return data.torque; }
uint16_t M3508::get_temperature() { return data.temperature; }
float M3508::get_target_angle() { return target_angle; }
float M3508::get_target_omega() { return target_omega; }
float M3508::get_target_torque() { return target_torque; }
float M3508::get_out() { return out; }
uint16_t M3508::get_out_max() { return out_max; }

void M3508::set_target_angle(float _target_angle)
{
	target_angle = _target_angle;
}
void M3508::set_target_omega(float _target_omega)
{
	target_omega = _target_omega;
}
void M3508::set_target_torque(float _target_torque)
{
	target_torque = _target_torque;
}
void M3508::set_out(float _out) { out = _out; }
#endif

/*========================COPYRIGHT(C) CCUT-DAMOCLES==========================*/
